#include "My_Inc/PHY/PHY_VSG.h"
void VSG_TOGI_Init(VSGTOGI *transform, float TOGI_k, float TOGI_T);
void VSG_SignalAnalysisr_Init(VSGSignalAnalysisr *v, float SampleFreq, float Threshold, uint32_t nsamplesMin,
                              uint32_t nsamplesMax);
void VSG_Integral_Init(VSGIntegral *Integral, float ts);
void VSG_PLLCalculate_Init(VSGPLL *PLL, float Kp, float Ki, float ts);
void VSG_PI_Init(VSGPI *pi, float Kp, float Ki, const float ts, float output_min, float output_max);
void VSG_Control_Init(VSG_Typedef *control, float D, float J, float ts, float Un, float VSG_Q_Kq, float VSG_Q_Kp)
{
    control->D = D;            // 阻尼系数
    control->J = J;            // 惯量
    control->Inv_J = 1.0f / J; // 惯量的倒数
    control->ts = ts;
    control->VSG_Q_Kq = VSG_Q_Kq;   // 无功下垂系数
    control->VSG_Q_Kp = VSG_Q_Kp;   // 无功幅值比例系数
    control->W = 2.0f * 50.0f * PI; // 内部角频率
    control->Un = Un;               // 无功下垂系数

    control->Pe = 0.0f; // 功率参考值和实际值
    control->Qe = 0.0f;
    control->wt = 0.0f;     // 相角
    control->E0 = 0.0f;     // 最终输出的幅值
    control->E = 0.0f;      // 最终合成的电
    control->Sine = 0.0f;   // 正弦值
    control->Cosine = 0.0f; // 余弦值

    control->dU = 0.0f;     // 预同步电压差值
    control->dTheta = 0.0f; // 预同步相位差

    VSG_PI_Init(&control->PI_U, 0.01f, 10.0f, ts, 0.0f, 50.0f);         // 电压预同步的 PI 控制器
    VSG_PI_Init(&control->PI_theta, 0.01f, 10.0f, ts, 0.0f, 2.0f * PI); // 相位预同步的 PI 控制器

    VSG_TOGI_Init(&control->TOGI_I, 1.414f, ts);
    VSG_TOGI_Init(&control->TOGI_V, 1.414f, ts);

    VSG_Integral_Init(&control->Integral1, ts); // 积分器1
    VSG_Integral_Init(&control->Integral2, ts); // 积分器
    VSG_Integral_Init(&control->Integral3, ts); // 积分器3

    VSG_SignalAnalysisr_Init(&control->SignalAnalysisr_VG, VSG_SAMPLE_RATE, 0.05f, 10, 100);
    VSG_SignalAnalysisr_Init(&control->SignalAnalysisr_E, VSG_SAMPLE_RATE, 0.05f, 10, 100);

    VSG_PLLCalculate_Init(&control->PLL, 2.0f, 5.0f, ts); // 锁相环
}
void VSG_SignalAnalysisr_Init(VSGSignalAnalysisr *v, float SampleFreq, float Threshold, uint32_t nsamplesMin,
                              uint32_t nsamplesMax) // 初始化结构体（带采样数限制）
{
    if (v == NULL)
        return;
    memset(v, 0, sizeof(VSGSignalAnalysisr));
    v->curr_sign = 0;
    v->SampleFreq = SampleFreq;   // 输入：信号采样频率（Hz）
    v->nsamplesMin = nsamplesMin; // 内部：周期采样数下限（防噪声干扰）
    v->nsamplesMax = nsamplesMax; // 内部：周期采样数上限（防异常信号）
    v->write_index = 0;
    // 输出参数
    v->Vrms = 0;     //  输出：电压有效值（RMS）
    v->SigFreq = 50; // 输出：信号频率（Hz）
    // 内部中间状态变量
    v->prev_sign = 0; // 内部：前一次采样的符号（0负/1正）
    v->curr_sign = 0; // 内部：当前采样的符号（0负/1正）
    v->nsamples = 0;  // 内部：当前周期内的采样数
    v->inv_nsamples = 1.0f / VSG_BUFFER_SIZE;
    // 内部变量
    v->Vacc_rms = 0;         // 累积的平方和值（用于计算 Vrms）本质上是一个中间变量
    v->curr_sample_norm = 0; // 当前采样值的归一化值（curr_sample_norm）
    v->buffer_size = VSG_BUFFER_SIZE;
}
void VSG_TOGI_Init(VSGTOGI *transform, float TOGI_k, float TOGI_T)
{
    if (transform == NULL)
        return;
    transform->TOGI_k = TOGI_k;        // TOGI增益
    transform->TOGI_T = TOGI_T * 0.5f; // TOGI时间常数
    transform->Integral1_Yn = 0.0f;    // 积分器1
    transform->Integral1_Xn = 0.0f;    // 积分器1
    transform->Integral1_Xn_1 = 0.0f;  // 积分器1

    transform->Integral2_Yn = 0.0f;   // 积分器1
    transform->Integral2_Xn = 0.0f;   // 积分器1
    transform->Integral2_Xn_1 = 0.0f; // 积分器1

    transform->Integral3_Yn = 0.0f;   // 积分器1
    transform->Integral3_Xn = 0.0f;   // 积分器1
    transform->Integral3_Xn_1 = 0.0f; // 积分器1

    transform->U1 = 0.0f; // 输出1
    transform->U2 = 0.0f; // 输出2
    transform->U3 = 0.0f; // 输出3

    transform->Alpha = 0.0f; // Alpha电压
    transform->Beta = 0.0f;  // Beta电压
    transform->D = 0.0f;     // D电压
    transform->Q = 0.0f;     // Q电压
}
void VSG_Integral_Init(VSGIntegral *Integral, float ts)
{
    if (Integral == NULL)
        return;
    Integral->ts_half = ts * 0.5f;
    Integral->Xn = 0.0f;
    Integral->Xn_1 = 0.0f;
    Integral->Yn = 0.0f;
}
void VSG_PI_Init(VSGPI *pi, float Kp, float Ki, const float ts, float output_min, float output_max)
{
    if (pi == NULL)
        return;
    pi->ERROR = 0.0f;
    pi->ERROR_Last = 0.0f;
    pi->Out = 0.0f;
    pi->A0 = (Kp + Ki * ts * 0.5f);
    pi->B0 = (Ki * ts * 0.5f - Kp);
}
void VSG_PLLCalculate_Init(VSGPLL *PLL, float Kp, float Ki, float ts)
{
    if (PLL == NULL)
        return;
    // PI 控制器参数
    PLL->Coeff_B0 = (Kp + Ki * ts * 0.5f); // 比例增益
    PLL->Coeff_B1 = (Ki * ts * 0.5f - Kp); // 积分增益
    PLL->Yn = 0.0f;                        // 上一次输出
    PLL->Xn = 0.0f;                        // 上一次输出
    PLL->Xn_1 = 0.0f;                      // 上一次输出
    // 相位和频率
    PLL->Theta = 0.0f; // 相位
    // 状态变量
    PLL->Integral_Xn = 0.0f;   // 上一次输入
    PLL->Integral_Xn_1 = 0.0f; // 上一次输入
    PLL->Integral_Yn = 0.0f;   // 上一次输出
    PLL->ts_half = 0.5f * ts;  // 0.5 * ts，用于计算
}

/*不带预同步单元 无电压 相位同步*/
// static inline void VSG_Control_Inline(VSG_Typedef *control, float Pref, float Qref, float Vo, float Io)
// {
//     /*分解出DQ分量*/
//     // VSG_TOGItoDQ_Inline(&control->TOGI_V, Vo, control->W, &control->Sine, &control->Cosine);
//     // VSG_TOGItoDQ_Inline(&control->TOGI_I, Io, control->W, &control->Sine, &control->Cosine);
//     VSG_TOGItoAphlaBeta_Inline(&control->TOGI_V, Vo, control->W); // 将电压从TOGI变换到Alpha-Beta
//     VSG_TOGItoAphlaBeta_Inline(&control->TOGI_I, Io, control->W); // 将电流从TOGI变换到Alpha-Beta
//     // 计算有功无功实际值
//     // float Pe = 0.5f *(control->TOGI_V.D * control->TOGI_I.D + control->TOGI_V.Q * control->TOGI_I.Q);
//     // float Qe = 0.5f *(control->TOGI_V.Q * control->TOGI_I.D - control->TOGI_V.D * control->TOGI_I.Q);
//     float Pe = 0.5f * (control->TOGI_V.Alpha * control->TOGI_I.Alpha + control->TOGI_V.Beta *
//     control->TOGI_I.Beta); float Qe = 0.5f * (control->TOGI_V.Beta * control->TOGI_I.Alpha -
//     control->TOGI_V.Alpha * control->TOGI_I.Beta);
//     // 更新有功无功实际值
//     control->Pe = Pe * 0.005f + control->Pe * 0.995f; // 低通滤波
//     control->Qe = Qe * 0.005f + control->Qe * 0.995f; // 低通滤波
//     /*有功调频环路 */
//     // 积分器1的计算 (角频率w的导数积分)
//     control->Integral1.Xn = ((Pref - control->Pe) / control->W - (control->D * control->Integral1.Yn)) *
//     control->Inv_J; control->Integral1.Yn += control->ts * 0.5f * (control->Integral1.Xn +
//     control->Integral1.Xn_1);
//     // 梯形积分 control->Integral1.Xn_1 = control->Integral1.Xn; // 移位
//                                                                                                      //
//                                                                                                      更新角频率W0
//     control->W = control->Integral2.Xn = control->Integral1.Yn + (float)(2.0f * 50.0f * PI);         // 100 * PI
//     // 积分器2的计算 (角频率w的积分得到角度wt)
//     control->Integral2.Yn += control->ts * 0.5f * (control->Integral2.Xn + control->Integral2.Xn_1); // 梯形积分
//     control->Integral2.Xn_1 = control->Integral2.Xn;                                                 // 移位
//     // 角度归一化到 [0, 2*PI) 范围
//     if (control->Integral2.Yn >= 2.0f * PI)
//         control->Integral2.Yn -= 2.0f * PI;
//     else if (control->Integral2.Yn < 0.0f)
//         control->Integral2.Yn += 2.0f * PI;

//     control->wt = control->Integral2.Yn; // 更新角度wt

//     /*无功调压环路 */
//     VSG_SignalAnalysisr_Inline(&control->SignalAnalysisr, Vo); // 信号分析器处理电压样本
//     control->Integral3.Xn =
//         (control->VSG_Q_Kp * (control->Un - control->SignalAnalysisr.Vrms)) + Qref - control->Qe;    // 无功调节
//     control->Integral3.Yn += control->ts * 0.5f * (control->Integral3.Xn + control->Integral3.Xn_1); // 梯形积分
//     control->Integral3.Xn_1 = control->Integral3.Xn;
//     control->E0 = control->Integral3.Yn * control->VSG_Q_Kq; // 计算最终的电压参考值

//     /*合成最终的电压参考值*/
//     Cordic_SinCos_Inline(control->wt, &control->Sine, &control->Cosine); // 计算正弦和余弦值
//     control->E = control->E0 * control->Sine;
// }